#include "MMMViewerWindow.h"
#include "ui_MMMViewerWindow.h"

#include "PluginHandler/PluginHandlerDialog.h"
#include "MotionHandlerFactory.h"
#include "SaveMotionDialog.h"

#include <MMM/MathTools.h>
#include <MMM/XMLTools.h>
#include <MMM/Motion/MotionReaderXML.h>
#include <MMM/Motion/MotionWriterXML.h>
#include <boost/bind.hpp>
#include <iostream>
#include <iomanip>
#include <ctime>
#include <qgl.h>
#include <iostream>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <Inventor/actions/SoLineHighlightRenderAction.h>
#include <Inventor/nodes/SoUnits.h>
#include <Inventor/Qt/SoQt.h>
#include <QCloseEvent>
#include <QMessageBox>
#include <QFileDialog>
#include <QSignalMapper>
#include <QDir>
#include "../common/HandleMotionsWithoutModel.h"

MMMViewerWindow::MMMViewerWindow(QWidget* parent) :
    QMainWindow(parent),
    sceneGraph(createSceneGraph()),
    floorVisualisation(createFloorVisualisation(sceneGraph)),
    progressTimer(new SoTimerSensor(progressTimerCallback, this)),
    pluginHandlerDialog(new PluginHandlerDialog(this)),
    sensorVisualisationHandler(new SensorVisualisationHandler(sceneGraph)),
    motionChanged(false),
    minTimestep(0.0f),
    maxTimestep(0.0f),
    framesPerSecond(30),
    currentTimestep(0.0f),
    lastTimestep(0.0f),
    velocity(1.0f),
    ui(new Ui::MMMViewerWindow)
{
    ui->setupUi(this);

    qRegisterMetaTypeStreamOperators<QList<float> >("QList<float>");
    setupViewer();

    setTitle();

    addMotionMenu();
    addOptionMenu();

    boost::shared_ptr<PluginHandler<MMM::MotionHandlerFactory> > motionPluginHandler(new PluginHandler<MMM::MotionHandlerFactory>("Motion Handler", MOTION_HANDLER_PLUGIN_LIB_DIR));
    motionPluginHandler->updateSignal.connect(boost::bind(&MMMViewerWindow::updateMotionHandler, this, _1));

    boost::shared_ptr<PluginHandler<MMM::SensorFactory> > sensorPluginHandler(new PluginHandler<MMM::SensorFactory>("Sensor", MMM::MotionReaderXML::getStandardLibPath()));
    sensorPluginHandler->updateSignal.connect(boost::bind(&MMMViewerWindow::updateMotion, this, _1));

    boost::shared_ptr<PluginHandler<MMM::SensorVisualisationFactory> > sensorVisualisationPluginHandler(new PluginHandler<MMM::SensorVisualisationFactory>("Sensor Visualisation", SENSOR_VISUALISATION_PLUGIN_LIB_DIR));
    sensorVisualisationPluginHandler->updateSignal.connect(boost::bind(&SensorVisualisationHandler::updateSensorVisualisation, sensorVisualisationHandler.get(), _1));

    QMenu* extraMenu = new QMenu(tr("Extra"), this);
    QAction* pluginAction = new QAction(tr("&Plugins..."), extraMenu);
    pluginHandlerDialog->addPluginHandler(motionPluginHandler);
    pluginHandlerDialog->addPluginHandler(sensorPluginHandler);
    pluginHandlerDialog->addPluginHandler(sensorVisualisationPluginHandler);
    motionPluginHandler->emitUpdate(); // need to be called after adding all other plugin handler because of order of plugins
    sensorPluginHandler->emitUpdate();
    sensorVisualisationPluginHandler->emitUpdate();
    connect(pluginAction, SIGNAL(triggered()), pluginHandlerDialog, SLOT(show()));
    extraMenu->addAction(pluginAction);
    this->menuBar()->addMenu(extraMenu);

    setupVelocityComboBox();

    ui->viewerLayout->addWidget(sensorVisualisationHandler->getSensorVisualisationTable());
    connect(sensorVisualisationHandler.get(), SIGNAL(sensorVisualisationLoaded(bool)), this, SLOT(sensorVisualisationLoaded(bool)));

    connect(ui->timestepSlider, SIGNAL(valueChanged(int)), this, SLOT(sliderMoved(int)));
    connect(ui->fpsSpinBox, SIGNAL(valueChanged(int)), this, SLOT(fpsChanged(int)));
    connect(ui->playStopButton, SIGNAL(clicked()), this, SLOT(playStopMotion()));

    ui->fpsSpinBox->setValue(framesPerSecond);

    createTimestampString();

    loadLastMotionFile();
}

SoSeparator* MMMViewerWindow::createSceneGraph() {
    SoSeparator* _sceneGraph = new SoSeparator();
    _sceneGraph->ref();

    SoUnits *u = new SoUnits();
    u->units = SoUnits::MILLIMETERS;
    _sceneGraph->addChild(u);
    return _sceneGraph;
}

SoSwitch* MMMViewerWindow::createFloorVisualisation(SoSeparator* sceneGraph) {
    SoSwitch* floorSwitch = new SoSwitch();
    floorSwitch->whichChild = SO_SWITCH_ALL;

    SoSeparator* _floorVisualisation = VirtualRobot::CoinVisualizationFactory::CreatePlaneVisualization(Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 1.0f), 10000.0f, 0);
    floorSwitch->addChild(_floorVisualisation);
    sceneGraph->addChild(floorSwitch);
    return floorSwitch;
}

void MMMViewerWindow::setupViewer() {
    viewer = new SoQtExaminerViewer(ui->viewer, "", TRUE, SoQtExaminerViewer::BUILD_POPUP);
    viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
    viewer->setAccumulationBuffer(true);
    viewer->setGLRenderAction(new SoLineHighlightRenderAction);
    viewer->setTransparencyType(SoGLRenderAction::BLEND);
    viewer->setFeedbackVisibility(true);
    viewer->setSceneGraph(sceneGraph);
    viewer->viewAll();
    setupCamera();
}

void MMMViewerWindow::setupCamera() {
    SoCamera* camera = viewer->getCamera();

    QList<float> qInitPosition;
    qInitPosition.append(-4.0f);
    qInitPosition.append(0.0f);
    qInitPosition.append(1.0f);
    QList<float> qPosition = settings.value("camera/position", QVariant::fromValue(qInitPosition)).value<QList<float> >();
    camera->position.setValue(SbVec3f(qPosition[0], qPosition[1], qPosition[2]));

    QList<float> qInitOrientation;
    qInitOrientation.append(0.5f);
    qInitOrientation.append(-0.5f);
    qInitOrientation.append(-0.5f);
    qInitOrientation.append(0.5f);
    QList<float> qOrientation = settings.value("camera/orientation", QVariant::fromValue(qInitOrientation)).value<QList<float> >();
    camera->orientation.setValue(SbRotation(qOrientation[0], qOrientation[1], qOrientation[2], qOrientation[3]));

    camera->nearDistance.setValue(settings.value("camera/nearDistance", 0.0185f).toFloat());

    camera->farDistance.setValue(settings.value("camera/farDistance", 9.5f).toFloat());

    camera->focalDistance.setValue(settings.value("camera/focalDistance", 4.5f).toFloat());
}

void MMMViewerWindow::setupVelocityComboBox() {
    for (int i = 1; i <= 8; i++) {
        float v = 0.25f * i;
        ui->velocityBox->addItem(QString::fromStdString(boost::lexical_cast<std::string>(v).append("x")), QVariant(v));
        if (std::abs(velocity - v) < 0.00001) ui->velocityBox->setCurrentIndex(i - 1);
    }
    connect(ui->velocityBox, SIGNAL(currentIndexChanged(int)), this, SLOT(setVelocity(int)));
}

void MMMViewerWindow::updateMotion(const std::map<std::string, boost::shared_ptr<MMM::SensorFactory> > &sensorFactories) {
    motionReader = MMM::MotionReaderXMLPtr(new MMM::MotionReaderXML(sensorFactories)); // TODO Reload?
    motionReader->handleMissingModelFilePath = handleMissingModelFilePath;
}

void MMMViewerWindow::updateMotionHandler(const std::map<std::string, boost::shared_ptr<MMM::MotionHandlerFactory> > &motionHandlerFactories) {
    bool resetMenus = false;
    for (auto it = motionHandler.begin(); it != motionHandler.end();) {
        if (motionHandlerFactories.find(it->first) == motionHandlerFactories.end()) {
            resetMenus = true;
            // TODO close motionHandler!
            // TODO remove Plugin Handler
            pluginHandlerDialog->removePluginHandler(it->second->getPluginHandlerID());
            it = motionHandler.erase(it);
        }
        else it++;
    }
    if (resetMenus) {
        importMotionMenu->clear();
        importMotionMenu->setDisabled(true);
        exportMotionMenu->clear();
        exportMotionMenu->setDisabled(true);
        addSensorMenu->clear();
        addSensorMenu->setDisabled(true);
        generalMenu->clear();
        generalMenu->setDisabled(true);
    }
    QSignalMapper* signalMapper = new QSignalMapper(this);
    std::vector<MMM::MotionHandlerPtr> importMotionHandler;
    for (const auto &motionHandlerFactory : motionHandlerFactories) {
        std::string handlerName = motionHandlerFactory.first;
        if (motionHandler.find(handlerName) != motionHandler.end()) {
            if (resetMenus) goto AddAction;
        } else {
            motionHandler[handlerName] = motionHandlerFactory.second->createMotionHandler(this);
            {
                boost::shared_ptr<IPluginHandler> pluginHandler = motionHandler[handlerName]->getPluginHandler();
                if (pluginHandler) pluginHandlerDialog->addPluginHandler(pluginHandler);
            }

            AddAction:
            MMM::MotionHandlerPtr handler = motionHandler[handlerName];
            QAction* handlerAction = new QAction(QString::fromStdString(handler->getDescription()), this);
            handlerAction->setShortcut(QString::fromStdString(handler->getShortCut()));
            connect(handlerAction, SIGNAL(triggered()), signalMapper, SLOT(map()));
            signalMapper->setMapping(handlerAction, QString::fromStdString(handlerName));
            switch (handler->getType()) {
            case MMM::MotionHandlerType::IMPORT:
                importMotionMenu->setEnabled(true);
                importMotionMenu->addAction(handlerAction);
                importMotionHandler.push_back(handler);
                break;
            case MMM::MotionHandlerType::EXPORT:
                if (motions.size() > 0) exportMotionMenu->setEnabled(true);
                exportMotionMenu->addAction(handlerAction);
                break;
            case MMM::MotionHandlerType::ADD_SENSOR:
                if (motions.size() > 0) addSensorMenu->setEnabled(true);
                addSensorMenu->addAction(handlerAction);
                break;
            case MMM::MotionHandlerType::GENERAL:
                if (motions.size() > 0) generalMenu->setEnabled(true);
                generalMenu->addAction(handlerAction);
                break;
            }
            connect(this, SIGNAL(timestepChanged(float)), handler.get(), SLOT(timestepChanged(float)));
            connect(handler.get(), SIGNAL(openMotions(MMM::MotionList)), this, SLOT(openMotions(MMM::MotionList)));
            connect(handler.get(), SIGNAL(jumpTo(float)), this, SLOT(jumpTo(float)));
            connect(handler.get(), SIGNAL(saveScreenshot(float, std::string, std::string)), this, SLOT(saveScreenshot(float, std::string, std::string)));
        }
    }

    for (MMM::MotionHandlerPtr imh : importMotionHandler) {
        for (const auto &mh : motionHandler) {
            mh.second->addImportMotionHandler(imh);
        }
    }

    connect(signalMapper, SIGNAL(mapped(const QString &)), this, SLOT(handleMotion(const QString &)));

    if (exportMotionMenu->actions().size() > 0) connect(this, SIGNAL(motionsOpened(bool)), exportMotionMenu, SLOT(setEnabled(bool)));
    if (addSensorMenu->actions().size() > 0) connect(this, SIGNAL(motionsOpened(bool)), addSensorMenu, SLOT(setEnabled(bool)));
    if (generalMenu->actions().size() > 0) connect(this, SIGNAL(motionsOpened(bool)), generalMenu, SLOT(setEnabled(bool)));

}

void MMMViewerWindow::addMotionMenu() {
    QMenu* motionMenu = new QMenu(tr("&Motion"), this);

    QAction* openMotionAction = new QAction(tr("&Open..."), motionMenu);
    openMotionAction->setShortcut(tr("Ctrl+O"));
    connect(openMotionAction, SIGNAL(triggered()), this, SLOT(openMotion()));
    motionMenu->addAction(openMotionAction);

    importMotionMenu = new QMenu(tr("&Import"), motionMenu);
    importMotionMenu->setDisabled(true);
    motionMenu->addMenu(importMotionMenu);

    QAction* saveMotionAction = new QAction(tr("&Save..."), motionMenu);
    saveMotionAction->setShortcut(tr("Ctrl+S"));
    saveMotionAction->setDisabled(true);
    connect(this, SIGNAL(motionsOpened(bool)), saveMotionAction, SLOT(setEnabled(bool)));
    connect(saveMotionAction, SIGNAL(triggered()), this, SLOT(saveMotion()));
    motionMenu->addAction(saveMotionAction);

    exportMotionMenu = new QMenu(tr("&Export"), motionMenu);
    exportMotionMenu->setDisabled(true);
    motionMenu->addMenu(exportMotionMenu);

    motionMenu->addSeparator();

    addSensorMenu = new QMenu(tr("&Add sensor"), motionMenu);
    addSensorMenu->setDisabled(true);
    motionMenu->addMenu(addSensorMenu);

    generalMenu = new QMenu(tr("&General"), motionMenu);
    generalMenu->setDisabled(true);
    motionMenu->addMenu(generalMenu);

    motionMenu->addSeparator();

    QAction* exitApplication = new QAction(tr("&Quit"), motionMenu);
    exitApplication->setShortcut(tr("Ctrl+Q"));
    connect(exitApplication, SIGNAL(triggered()), this, SLOT(closeWindow()));
    motionMenu->addAction(exitApplication);

    this->menuBar()->addMenu(motionMenu);
}

void MMMViewerWindow::addOptionMenu() {
    QMenu* optionMenu = new QMenu(tr("&Options"), this);

    QAction* showVisualisationTableAction = new QAction(tr("&Show Visualisation Table"), optionMenu);
    showVisualisationTableAction->setCheckable(true);
    bool visualisationTable = settings.value("mainwindow/visualisationTable", true).toBool();
    showVisualisationTableAction->setChecked(visualisationTable);
    displayVisualisationTable(visualisationTable);
    showVisualisationTableAction->setShortcut(tr("Ctrl+T"));
    connect(showVisualisationTableAction, SIGNAL(triggered(bool)), this, SLOT(displayVisualisationTable(bool)));
    optionMenu->addAction(showVisualisationTableAction);

    QAction* showFloorAction = new QAction(tr("&Show Floor"), optionMenu);
    showFloorAction->setCheckable(true);
    bool floor = settings.value("mainwindow/floor", true).toBool();
    showFloorAction->setChecked(floor);
    showFloor(floor);
    showFloorAction->setShortcut(tr("Ctrl+F"));
    connect(showFloorAction, SIGNAL(triggered(bool)), this, SLOT(showFloor(bool)));
    optionMenu->addAction(showFloorAction);

    optionMenu->addSeparator();

    QAction* enableAntiAliasingAction = new QAction(tr("&Enable AntiAliasing"), optionMenu);
    enableAntiAliasingAction->setCheckable(true);
    bool antiAliasing = settings.value("mainwindow/antiAliasing", false).toBool();
    enableAntiAliasingAction->setChecked(antiAliasing);
    setAntiAliasing(antiAliasing);
    enableAntiAliasingAction->setShortcut(tr("Ctrl+A"));
    connect(enableAntiAliasingAction, SIGNAL(triggered(bool)), this, SLOT(setAntiAliasing(bool)));
    optionMenu->addAction(enableAntiAliasingAction);

    QAction* enableInterpolationAction = new QAction(tr("&Enable Interpolation"), optionMenu);
    enableInterpolationAction->setCheckable(true);
    bool interpolation = settings.value("mainwindow/interpolation", false).toBool();
    enableInterpolationAction->setChecked(interpolation);
    setInterpolationEnabled(interpolation);
    connect(enableInterpolationAction, SIGNAL(triggered(bool)), this, SLOT(setInterpolationEnabled(bool)));
    optionMenu->addAction(enableInterpolationAction);

    this->menuBar()->addMenu(optionMenu);
}

MMMViewerWindow::~MMMViewerWindow() {
    delete ui;
}

void MMMViewerWindow::showWindow() {
    std::cout << "Starting MMMViewer" << std::endl;
    SoQt::show(this);
    SoQt::mainLoop();
}

void MMMViewerWindow::closeEvent(QCloseEvent *event) {
    event->setAccepted(closeWindow());
}

bool MMMViewerWindow::closeWindow() {
    if (motionChanged && QMessageBox::Yes == QMessageBox::question(this, tr("Unsaved changes"), tr("The motion contains unsaved changes. Do you want to save?"), QMessageBox::No|QMessageBox::Yes)) {
        saveMotion();
        return false;
    } else {
        std::cout << "Closing MMMViewer" << std::endl;
        saveCameraSettings();
        settings.setValue("motion/backup", false);
        this->close();
        return true;
    }
}

void MMMViewerWindow::setTitle(const std::string &motionFilePath) {
    std::string windowTitle = MMM::XML::getFileName(motionFilePath);
    if (!windowTitle.empty()) windowTitle.append(" - ");
    windowTitle.append("MMMViewer");
    this->setWindowTitle(QString::fromUtf8(windowTitle.c_str()));
}

bool MMMViewerWindow::openMotion() {
    std::string motionFilePath = QFileDialog::getOpenFileName(this, tr("Open motion"), settings.value("motion/searchpath", "").toString(), tr("Motion files (*.xml)")).toStdString();
    return openMotion(motionFilePath);
}

bool MMMViewerWindow::openMotion(const std::string &motionFilePath, bool ignoreError) {
    if (!motionFilePath.empty()) {
        try {
            settings.setValue("motion/searchpath", QString::fromStdString(MMM::XML::getPath(motionFilePath)));
            MMM::MotionList motions = motionReader->loadAllMotions(motionFilePath);
            if (openMotions(motions, ignoreError, motionFilePath)) {
                settings.setValue("motion/path", QString::fromStdString(motionFilePath));
                settings.setValue("motion/type", QString::fromStdString("MMM"));
                motionChanged = false;
                settings.setValue("motion/backup", false);
                return true;
            }
        } catch (MMM::Exception::MMMException &e) {
            if (!ignoreError) errorMessageBox("Could not open motion '" + motionFilePath + "'! " + e.what());
        }
    }
    return false;
}

bool MMMViewerWindow::openMotions(MMM::MotionList motions, bool ignoreError, const std::string &motionFilePath) {
    if (motions.size() > 0) {
        bool hasSensor = false;
        for (auto motion : motions) hasSensor |= motion->hasSensor();
        if (!hasSensor && !ignoreError) errorMessageBox("Motion not loaded, because no sensors available!");
        else if (calculateTimesteps(motions)) {
            stopTimer();
            this->motions = motions;
            if (motionFilePath.empty()) {
                setTitle(settings.value("motion/path").toString().toStdString());
                motionChanged = true;
                settings.setValue("motion/backup", true);
                saveMotionBackup();
            }
            else setTitle(motionFilePath);
            sensorVisualisationHandler->loadSensorVisualisation(motions);
            emit motionsOpened(true);
            return true;
        }
    }
    return false;
}

void MMMViewerWindow::sensorVisualisationLoaded(bool loaded) {
    if (loaded) {
        ui->currentTimestep->setText(QString::number(currentTimestep));
        ui->maxTimestep->setText(QString::number(maxTimestep));
        ui->timestepSlider->setEnabled(true);
        ui->timestepSlider->setRange(minTimestep * framesPerSecond, maxTimestep * framesPerSecond);
        ui->timestepSlider->setSliderPosition(currentTimestep * framesPerSecond);
        ui->playStopButton->setEnabled(true);
        sensorVisualisationHandler->changeTimestep(currentTimestep);
    } else {
        ui->currentTimestep->setText(QString::number(0.0f));
        ui->maxTimestep->setText(QString::number(0.0f));
        ui->timestepSlider->setEnabled(false);
        ui->timestepSlider->setRange(0,0);
        ui->timestepSlider->setSliderPosition(0);
        ui->playStopButton->setEnabled(false);
    }

}

void MMMViewerWindow::handleMotion(const QString &handlerName) {
    MMM::MotionHandlerPtr handler = motionHandler[handlerName.toStdString()];
    stopTimer();
    handler->handleMotion(motions);
}

void MMMViewerWindow::saveMotion() {
    stopTimer();
    SaveMotionDialog* saveMotionDialog = new SaveMotionDialog(motions, this);
    connect(saveMotionDialog, SIGNAL(openMotion(std::string)), this, SLOT(openMotion(std::string)));
    connect(saveMotionDialog, SIGNAL(motionSaved()), this, SLOT(motionSaved()));
    saveMotionDialog->open();
}

void MMMViewerWindow::motionSaved() {
    motionChanged = false;
    settings.setValue("motion/backup", false);
}

bool MMMViewerWindow::calculateTimesteps(MMM::MotionList motions) {
    float min = std::numeric_limits<float>::max();
    float max = 0.0f;

    for (MMM::MotionPtr motion : motions) {
        float maxMotionTimestep = motion->getMaxTimestep();
        if (max < maxMotionTimestep) max = maxMotionTimestep;

        float minMotionTimestep = motion->getMinTimestep();
        if (min > minMotionTimestep) min = minMotionTimestep;
    }

    if (min < max + 0.000001) {
        minTimestep = min;
        currentTimestep = minTimestep;
        lastTimestep = minTimestep;
        maxTimestep = max;
        return true;
    }

    return false;
}

void MMMViewerWindow::setAntiAliasing(bool value) {
    settings.setValue("mainwindow/antiAliasing", value);
    viewer->setAntialiasing(value, value ? 8 : 1);
}

void MMMViewerWindow::showFloor(bool value) {
    settings.setValue("mainwindow/floor", value);
    if (value) floorVisualisation->whichChild = SO_SWITCH_ALL;
    else floorVisualisation->whichChild = SO_SWITCH_NONE;
}

void MMMViewerWindow::displayVisualisationTable(bool value) {
    settings.setValue("mainwindow/visualisationTable", value);
    sensorVisualisationHandler->displaySensorVisualisationTable(value);
}

void MMMViewerWindow::setInterpolationEnabled(bool value) {
    settings.setValue("mainwindow/interpolation", value);
    sensorVisualisationHandler->setInterpolation(value);
}

void MMMViewerWindow::progressTimerCallback(void* data, SoSensor* sensor)
{
    MMMViewerWindow* viewerWindow = static_cast<MMMViewerWindow*>(data);
    viewerWindow->progressMotion();
}

void MMMViewerWindow::progressMotion() {
    currentTimestep = lastTimestep + MMM::Math::roundf((SbTime::getTimeOfDay() - progressTimer->getBaseTime()).getValue()) * velocity;
    if (currentTimestep > maxTimestep) {
        progressTimer->unschedule();
        currentTimestep = maxTimestep;
        lastTimestep = minTimestep;
        jumpTo(currentTimestep);
        ui->playStopButton->setText("Play");
        if (ui->repeatMotionCheckBox->isChecked()) {
            currentTimestep = minTimestep;
            playStopMotion();
        }
    } else {
        jumpTo(currentTimestep);
    }
}

void MMMViewerWindow::fpsChanged(int newValue)
{
    framesPerSecond = newValue;
    ui->timestepSlider->setRange(minTimestep * framesPerSecond, maxTimestep * framesPerSecond);
    ui->timestepSlider->setSliderPosition(currentTimestep * framesPerSecond);

    if (progressTimer->isScheduled()) {
        progressTimer->unschedule();
        progressTimer->setInterval(SbTime(1.0f / framesPerSecond));
        progressTimer->schedule();
    }
}

void MMMViewerWindow::sliderMoved(int position)
{
    if (!progressTimer->isScheduled()) {
        currentTimestep = ((float) position) / framesPerSecond;
        lastTimestep = currentTimestep;
        jumpTo(currentTimestep);
    }
}

void MMMViewerWindow::setVelocity(int index) {
    SoCamera* camera = viewer->getCamera();
    std::cout << camera->nearDistance.getValue() << " " << camera->farDistance.getValue() << " " << camera->focalDistance.getValue() << " " << camera->aspectRatio.getValue() << std::endl;
    if (progressTimer->isScheduled()) {
        progressTimer->unschedule();
        lastTimestep = currentTimestep;
        progressTimer->setBaseTime(SbTime::getTimeOfDay());
        progressTimer->schedule();
    }
    velocity = ui->velocityBox->itemData(index).toFloat();
}

void MMMViewerWindow::jumpTo(float timestep)
{
    std::cout << "Jump to timestep " << timestep << std::endl;
    ui->currentTimestep->setText(QString::number(timestep));
    disconnect(ui->timestepSlider, SIGNAL(valueChanged(int)), this, SLOT(sliderMoved(int))); // dont emit SliderMoved, because this would execute jumpTo (with an inaccurate position) !
    ui->timestepSlider->setSliderPosition(timestep * framesPerSecond + 0.001); // adding e-3 because of float to int rounding issues
    emit timestepChanged(timestep);
    connect(ui->timestepSlider, SIGNAL(valueChanged(int)), this, SLOT(sliderMoved(int)));
    sensorVisualisationHandler->changeTimestep(timestep);
}

void MMMViewerWindow::playStopMotion()
{
    if (!progressTimer->isScheduled()) {
        progressTimer->setInterval(SbTime(1.0 / framesPerSecond));
        progressTimer->setBaseTime(SbTime::getTimeOfDay());
        progressTimer->schedule();
        ui->playStopButton->setText("Stop");
    } else {
        lastTimestep = currentTimestep;
        progressTimer->unschedule();
        ui->playStopButton->setText("Play");
    }
}

void MMMViewerWindow::saveScreenshot(float timestep, const std::string &directory, const std::string &name)
{
    std::string fileNameStr = (name.empty() ? "MMMViewerWindow_" + boost::lexical_cast<std::string>(MMM::Math::roundf(timestep)) : name) + ".png";
    QString fileName = (!directory.empty()) ? QString::fromStdString(directory + "/" + fileNameStr) : QString::fromStdString(fileNameStr);

    if (!std::filesystem::exists(directory)) {
        std::filesystem::create_directory(directory);
    }

    viewer->getSceneManager()->render();
    viewer->getSceneManager()->scheduleRedraw();
    QGLWidget* w = (QGLWidget*) viewer->getGLWidget();

    QImage i = w->grabFrameBuffer();
    bool bRes = i.save(fileName, "PNG");
    if (bRes) std::cout << "Wrote image " << name << " on timestep " << timestep << std::endl;
    else std::cout << "Failed writing image on timestep " << timestep << std::endl;
}

void MMMViewerWindow::stopTimer() {
    if (progressTimer->isScheduled()) playStopMotion();
}

void MMMViewerWindow::errorMessageBox(const std::string &message) {
    QMessageBox* msgBox = new QMessageBox(this);
    msgBox->setText(QString::fromStdString(message));
    MMM_ERROR << message << std::endl;
    msgBox->exec();
}

void MMMViewerWindow::loadLastMotionFile() {
    if (settings.value("motion/backup", false).toBool()) {
        std::string path = settings.value("motion/backup/path", "").toString().toStdString();
        if (!path.empty()) {
            MMM_INFO << "Loading backup motion from " << path << std::endl;
            if (!openMotion(path, true))
                MMM_INFO << "Could not load last backup motion from " << path << std::endl;
        }
    }
    else if (settings.value("motion/type", "").toString() == "MMM") {
        std::string path = settings.value("motion/path", "").toString().toStdString();
        if (!path.empty()) {
            MMM_INFO << "Loading last used motion from " << path << std::endl;
            if (!openMotion(path, true))
                MMM_INFO << "Could not load last motion from " << path << std::endl;
        }
    }
}

void MMMViewerWindow::saveCameraSettings() {
    SoCamera* camera = viewer->getCamera();

    QList<float> qPosition;
    for (float pos : std::vector<float>(camera->position.getValue().getValue(), camera->position.getValue().getValue() + 3)) qPosition.append(pos);
    settings.setValue("camera/position", QVariant::fromValue(qPosition));

    QList<float> qOrientation;
    for (float ori : std::vector<float>(camera->orientation.getValue().getValue(), camera->orientation.getValue().getValue() + 4)) qOrientation.append(ori);
    settings.setValue("camera/orientation", QVariant::fromValue(qOrientation));

    settings.setValue("camera/nearDistance", camera->nearDistance.getValue());
    settings.setValue("camera/farDistance", camera->farDistance.getValue());
    settings.setValue("camera/focalDistance", camera->focalDistance.getValue());
}

void MMMViewerWindow::createTimestampString() {
    auto t = std::time(nullptr);
    auto tm = *std::localtime(&t);
    char buffer[80];
    std::strftime(buffer,sizeof(buffer),"%d.%m.%Y-%H:%M:%S",&tm);
    std::stringstream ss;
    ss << buffer;
    timestamp = ss.str();
}

void MMMViewerWindow::saveMotionBackup() {
    QString tempPath = QDir::tempPath();
    std::string path = tempPath.toStdString() + "/h2t-mmmviewer-motion-backups";
    std::filesystem::path dir(path);
    std::filesystem::create_directory(dir);
    MMM::MotionWriterXMLPtr motionWriter(new MMM::MotionWriterXML());
    try {
        std::string motionFilePath = path + "/motion-backup-" + timestamp + ".xml";
        motionWriter->writeMotion(motions, motionFilePath);
        settings.setValue("motion/backup/path", QString::fromStdString(motionFilePath));
    } catch(MMM::Exception::MMMException &e) {
        settings.setValue("motion/backup", false);
    }
}
